/*
 * exploration.cpp
 *
 *  Created on: Nov 14, 2012
 *      Author: Matthijs
 */
#include <behavior/exploration.h>


Exploration::Exploration (ros::NodeHandle n) :
	nodeHandle(n),
	grid()
{
	priority   			= 0.3;
	active 				= true;
	distance_subscriber = nodeHandle.subscribe("/sensor/ir", 2, & Exploration::distance_callback, this);

	ROS_INFO("Exploration behavior ready.");
}


/**
 * 	Parse information about available directions
 */
void Exploration::distance_callback (const sensor::IR::ConstPtr & msg)
{
	setValid(msg);

}


/**
 *  Set the valid direction
 */
void Exploration::setValid (const sensor::IR::ConstPtr & msg)
{
	// Store IR sensor readouts
	frontRight  = msg -> dists[6];
	frontLeft	= msg -> dists[7];
	rightFront  = msg -> dists[5];
	rightBack	= msg -> dists[4];
	leftFront  	= msg -> dists[0];
	leftBack 	= msg -> dists[1];
	backRight  	= msg -> dists[3];
	backLeft	= msg -> dists[2];
}


/**
 *	Act on the exploration behavior
 *
 *	This will try to explore the maze. Just drive forward and let the other
 *	behaviors handle walls. Once we know more about what regions are not
 *	explored, this behavior can be made more complex.
 *
 *
 */
bool Exploration::act (ros::Publisher publisher)
{
    ROS_ERROR("Exploring..");
    driver::Move move;
	move.distance 	= 0.1f;
	move.angle 		= 0.0;

	if ((leftFront > 0.25 && leftBack > 0.25) || (leftFront < 0.01 && leftBack < 0.01)) {
		ROS_INFO("Left wall went missing. Moving a bit forward.");
		move.distance = 0.02f;
		publisher.publish(move);

		double start = ros::Time::now().toSec();
		ros::Rate loop_rate(10);
		while (ros::ok()) {
			if (ros::Time::now().toSec() - start > 2.0f) {
				break;
			}
			loop_rate.sleep();
		}

		ROS_INFO("Assume left wall still gone. Turning left.");
		move.angle = M_PI / 2.0f;
		move.distance = 0.0f;
		publisher.publish(move);
		while (ros::ok()) {
			if (ros::Time::now().toSec() - start > 5.5f) {
				break;
			}
			loop_rate.sleep();
		}

		move.angle    = 0.0f;
		move.distance = 0.2f;
		publisher.publish(move);

		while (ros::ok()) {
			if (ros::Time::now().toSec() - start > 7.5f) {
				break;
			}
			loop_rate.sleep();
		}
	} else if (leftFront > 0.13 && leftBack > 0.13) {
		move.angle 		= 0.05f;
	}

	ROS_INFO("In [%d, %d]. Moving with (%f, %f)", x, y, move.distance, move.angle);

	publisher.publish(move);
    return true;
}
